#include "Scene.h"
#include "modeltodraw.h"

#include <QGraphicsItem>
#include <QPixmap>
#include <QPainter>
#include <QPaintEngine>
#include <QtOpenGL>

#include <AR/ar.h>
#include <AR/video.h>
#include <AR/config.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>
#include <coldet.h>

using namespace std;

bool first = false;
bool bingo = false;
static double matrix[16] = {0.0f};
static float fmatrix[16] = {0.0f};
static double leftDiff[7] = {0.0f};
static double rightDiff[7] = {0.0f};
double leftOne = 0.01;
double rightOne = 0.01;
double liftedOne = 0;
GLUquadricObj *quadric = gluNewQuadric();
int wid = 640;
int hei = 480;
ModelToDraw *left_controller;
ModelToDraw *right_controller;
ModelToDraw *cubes[4];
ModelToDraw *target;
int counting = -2;
float red[4] = {1.0f, 0.0f, 0.0f, 1.0f };
float blue[4] = {0.0f, 0.0f, 1.0f, 1.0f };
float green[4] = {0.0f, 1.0f, 0.0f, 1.0f };
QPen scorepen(Qt::red);
QFont f ("Courier", 14, QFont::Bold);
QFont c ("Courier", 16, QFont::Bold);

/**********************
 * Constructor
 */
CScene::CScene(QObject *parent) : QGraphicsScene(parent)
{
    StartCapture(wid, hei);
    m_NyART = new ARToolkitAPI();
    m_NyART->ARInit(640, 480);
    gluQuadricDrawStyle(quadric, GLU_FILL);
    gluQuadricNormals(quadric, GLU_SMOOTH);
    gluQuadricOrientation(quadric, GLU_OUTSIDE);
    left_controller = new ModelToDraw("include/penguin.3ds", 0.4);
    right_controller = new ModelToDraw("include/penguin.3ds", 0.4);
    target = new ModelToDraw("include/penguin.3ds", 0.60);
    countup = new QTimer();
    countup->setInterval(1000);
    countup->setSingleShot(false);
    connect(countup , SIGNAL(timeout()), this, SLOT(timeOut()));

}
// end CSene();


/**********************
 * Destructor
 */
CScene::~CScene()
{
    left_controller->~ModelToDraw();
    right_controller->~ModelToDraw();
    StopCapture();
}
// end ~CScene()


void CScene::timeOut(){
    counting++;
}

void CScene::startStop(int hmmm){
    if(hmmm == 0){
        countup->start();
    } else if(hmmm == 1){
        countup->stop();
        counting = -2;
    }
}

/**********************
 * Updates the values from the dynamometers - 0% ~ 100% and over
 */
void CScene::ChangeChildValues(double l, double r){
    leftOne = l/100;
    rightOne = r/100;
    if(leftOne <= 0) leftOne = 0.01;
    if(rightOne <= 0) rightOne = 0.01;
}
// end ChangeChildvalues()


/**********************
 * Start video feed
 */
bool CScene::StartCapture(int width, int height)
{
    // Start capture
    m_OpenCV.StartCapture(width, height);

    QTimer::singleShot(50, this, SLOT(update()));

    return true;
}
// end StartCapture()


/**********************
 * Stop video feed
 */
bool CScene::StopCapture()
{
    m_OpenCV.StopCapture();
    return true;
}
// end StopCapture()


/**********************
 * Controls lighting of the scene
 */
void lightup()
{
    glShadeModel(GL_SMOOTH);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glFrontFace(GL_CCW);
    float gambient[]={0.2,0.2,0.2,1.0};
    float lightpos[]={1.0,1.0,1.0,0.0};
    float lambient[]={1.0,1.0,1.0,1.0};
    float ldiffuse[]={0.8,0.8,0.8,1.0};
    float lspecular[]={0.3,0.3,0.3,1.0};
    glLightfv(GL_LIGHT0,GL_DIFFUSE,ldiffuse);
    glLightfv(GL_LIGHT0,GL_AMBIENT,lambient);
    glLightfv(GL_LIGHT0,GL_SPECULAR,lspecular);
    glLightfv(GL_LIGHT0,GL_POSITION,lightpos);
    glLightModelfv(GL_LIGHT_MODEL_AMBIENT,gambient);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
}
// end lightup()


void CScene::drawForeground(QPainter *painter, const QRectF &rect){
        painter->setPen(scorepen);
        painter->setFont(f);
        painter->drawText(-23, -rect.height()/2 +14, "TIME");

        if(counting < 1){

        } else {
        painter->setFont(c);
        painter->drawText((counting > 11 ? -10 : -5), -rect.height()/2 + 30, QString::number(counting));
        }


//        else if (finalcountdown < (gametime-3))
//        {
//            if(finalcountdown < 11) painter->setPen(countdownpen);
//            QFont c ("Courier", 16, QFont::Bold);
//            painter->setFont(c);
//            painter->drawText((finalcountdown > 11 ? -10 : -5), -heighty/2 + 30, QString::number(finalcountdown));
//            if(finalcountdown == 0)
//            {
//                countdownpen.setColor(randomColor());
//                painter->setPen(countdownpen);
//                QFont cdown ("Times", 60, QFont::Bold);
//                painter->setFont(cdown);
//                painter->drawText(-250, 25, "GAME OVER");
//                if(gameOverCount == 0) emit gameOver();
//                if(gameOverCount++ > 200) whatsaHappenin(0);
//                if(player1)
//                    if(rightscore > highscore){
//                        highscore = rightscore;
//                        emit newScore(1, highscore);
//                    }
//                playSound(7, 0);
//                playSound(3, 6);
//            }
//        }
//        else if(finalcountdown > (gametime-3))
//        {
//            countdownpen.setColor(randomColor());
//            painter->setPen(countdownpen);
//            QFont cdown ("Times", (finalcountdown - (gametime-4)) * 40, QFont::Bold);
//            painter->setFont(cdown);
//            painter->drawText(-5 - (finalcountdown - (gametime-4)) * 10, (finalcountdown - (gametime-4)) * 20, QString::number(finalcountdown - (gametime-3)));
//        }
//        else if (finalcountdown > 1)
//        {
//            countdownpen.setColor(randomColor());
//            painter->setPen(countdownpen);
//            QFont cdown ("Times", 60, QFont::Bold);
//            painter->setFont(cdown);
//            painter->drawText(-110, 25, "START");
//        }


}

/**********************
 * Controls all rendering of the scene
 */
void CScene::drawBackground(QPainter *painter, const QRectF &rect)
{
    if(!first){
        initializeBricks();
        first = true;
        glLoadMatrixd(target->getMatrix());
        glRotated(-90, 1, 0, 0);
        glGetDoublev(GL_MODELVIEW_MATRIX, target->getMatrix());
    }

    // Check if current painter is a OpenGL painter
    if((painter->paintEngine()->type() != QPaintEngine::OpenGL2) &&
            (painter->paintEngine()->type() != QPaintEngine::OpenGL))
    {
        return;
    }


    // Set clear color to white
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    // Set clear depth bit
    glClearDepth(1.0f);
    // Clear screen
    glDepthFunc(GL_LEQUAL);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Grab image from the webcam
    IplImage *cvimage = m_OpenCV.QueryFrame();

    // resize the picture to fill the screen. Used for window resize.
    if(cvimage != NULL)
    {
        IplImage *cvimage2 = cvCreateImage(cvSize(rect.width(), rect.height()), cvimage->depth, cvimage->nChannels);
        cvResize(cvimage, cvimage2, CV_INTER_LINEAR);
        glDrawPixels(cvimage2->width, cvimage2->height, GL_RGB, GL_UNSIGNED_BYTE, cvimage2->imageData);
        cvReleaseImage(&cvimage2);
        cvimage2 = NULL;
    }

    //glDepthFunc(GL_LEQUAL);
    lightup();
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

    bool isMarkerDetected = false;

    if(m_OpenCV.GetBGRAImage() != NULL)
    {
        // Is marker presence on the image?
        isMarkerDetected = m_NyART->FindShit(m_OpenCV.GetImage());

        if(isMarkerDetected)
        {

            glMatrixMode(GL_PROJECTION);
            glLoadIdentity();
            gluPerspective(45.0f, static_cast<float>(640) / static_cast<float>(480), 0.1f, 1000.0f);

            glMatrixMode(GL_MODELVIEW);
            glLoadIdentity();

            ///////////////////////// for model one
            glLoadMatrixd(m_NyART->GetTransformationMatrix(matrix, 0));
            DrawOtherThings(left_controller->model, left_controller->scale, leftOne);
            copyArrayd(matrix, fmatrix, 16);
            left_controller->collisionmodel->setTransform(fmatrix);
            getDifference(leftDiff, matrix);

            glLoadMatrixd(m_NyART->GetTransformationMatrix(matrix, 1));
            DrawOtherThings(right_controller->model, right_controller->scale, rightOne);
            copyArrayd(matrix, fmatrix, 16);
            right_controller->collisionmodel->setTransform(fmatrix);
            getDifference(rightDiff, matrix);

           // int i = 1;
            for(int i = 0; i < 4; i++){
            double *matrixTemp = cubes[i]->getMatrix();
                if((left_controller->collisionmodel->collision(cubes[i]->collisionmodel) && right_controller->collisionmodel->collision(cubes[i]->collisionmodel)) && (leftOne > 0.5 && rightOne > 0.5)){
                    glLoadMatrixd(matrixTemp);

                    glTranslatef((-leftDiff[0] + -rightDiff[0])/2, (-leftDiff[1] + -rightDiff[1])/2, (-leftDiff[2] + -rightDiff[2])/2);
                    glGetDoublev(GL_MODELVIEW_MATRIX, matrixTemp);

                    copyArrayd(matrixTemp, fmatrix, 16);
                    DrawOtherThings(cubes[i]->model, cubes[i]->scale, 1);
                    cubes[i]->collisionmodel->setTransform(fmatrix);
                } else {
                    glLoadMatrixd(matrixTemp);

                    copyArrayd(matrixTemp, fmatrix, 16);
                    DrawOtherThings(cubes[i]->model, cubes[i]->scale, -1);
                    cubes[i]->collisionmodel->setTransform(fmatrix);
                }
            }

            bingo = false;

            glLoadMatrixd(target->getMatrix());
            DrawOtherThings(target->model, target->scale, 0.4);
            //check for collision now before you render
            checkCollisions();

        }
    }
    // Timer will update our scene in 50 milliseconds
    QTimer::singleShot(50, this, SLOT(update()));
}
// end drawBackground()



/**********************
 *
 */
void CScene::initializeBricks(){
    int countinit = 0;
    double x = -3;
    double y = -2;
    double z = -8.0;
    for(int i = 0; i < 4; i++){
        cubes[i] = new ModelToDraw("include/penguin.3ds", 0.4);
        cubes[i]->setXYZ(x, y, z);
        x += 6;
        countinit ++;
        if(countinit == 2){
            x = -3;
            y = 2;
        }
        glLoadMatrixd(cubes[i]->getMatrix());
        glRotated(-90, 1, 0, 0);
        //glTranslated(1, 0, 0);
        glGetDoublev(GL_MODELVIEW_MATRIX, cubes[i]->getMatrix());
    }
    for(int i = 0; i < 4; i++){
        qDebug() << cubes[i];
    }

}
//end goHome()


/**********************
 * gets differences of translation, and other things (later) of object passed to it.
 */
void CScene::getDifference(double *trans, double *eeeek){
    if(trans[6] == 1){
        trans[0] = trans[3] - eeeek[12];
        trans[1] = trans[4] - eeeek[13];
        trans[2] = trans[5] - eeeek[14];
    } else {
        if(eeeek[12] != 0 && eeeek[13] != 0 && eeeek[14] != 0){
            trans[6] = 1;
            qDebug() << "here";
        }
    }
    trans[3] = eeeek[12];
    trans[4] = eeeek[13];
    trans[5] = eeeek[14];
}
// end checkCollisions()


/**********************
 * Checks for collisions, and handles them
 */
void CScene::checkCollisions(){
    if(left_controller->collisionmodel->collision(right_controller->collisionmodel)){
        qDebug() << "hit!!!";
    } else {
        qDebug() << "not hit!!!";
    }
}
// end checkCollisions()


/**********************
 * Draws the model to scale
 */
void CScene::DrawOtherThings(object3ds *todraw, float modelScale, float readyYet)
{
    if(readyYet < 0) glMaterialfv(GL_FRONT, GL_AMBIENT, blue);
    else glMaterialfv(GL_FRONT, GL_AMBIENT, (readyYet < 0.5 ? red : green));
    gluSphere(quadric, modelScale * 1.1, 20, 20);

    glScalef(modelScale, modelScale, modelScale);
    long meshcount;
    for(meshcount=0;meshcount<todraw->model->nmeshes;meshcount++)
    {
        todraw->face=todraw->mesh[meshcount]->faces;
        for(long i=0;i<todraw->mesh[meshcount]->nfaces;i++)
        {
            glMaterialfv(GL_FRONT,GL_DIFFUSE,todraw->material[todraw->face[i].material]->diffuse);
            glMaterialfv(GL_FRONT,GL_AMBIENT,todraw->material[todraw->face[i].material]->ambient);
            glMaterialfv(GL_FRONT,GL_SPECULAR,todraw->material[todraw->face[i].material]->specular);
            glBegin(GL_TRIANGLES);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[0]]);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[1]]);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[2]]);
            glEnd();
        }
    }
}
// end DrawOtherThings()


/**********************
 * Copies elements of a double array to float array
 */
void CScene::copyArrayd(double *source, float *destination, int howmany){
    for(int i = 0; i < howmany; i++){
        destination[i] = source[i];
    }
}
// end copyArray()

void CScene::copyArray(double *source, double *destination, int howmany){
    for(int i = 0; i < howmany; i++){
        destination[i] = source[i];
    }
}



